#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include <chrono>

using NavigatorToPose = nav2_msgs::action::NavigateToPose;
using namespace std::chrono_literals;

class NavToPoseClient : public rclcpp::Node{
private:
    rclcpp_action::Client<NavigatorToPose>::SharedPtr m_nav_client;

public:
    NavToPoseClient():Node("navigator_node"){
        RCLCPP_INFO(this->get_logger(),"节点navigator_node已启动...");
        m_nav_client = rclcpp_action::create_client<NavigatorToPose>(this,"navigate_to_pose");
    }

    void send_goal(){
        while(!m_nav_client->wait_for_action_server(5s))
        {
            RCLCPP_WARN(this->get_logger(),"等待Action服务上线");
        }
        auto goal_msg = NavigatorToPose::Goal();
        goal_msg.pose.header.stamp = this->get_clock()->now();
        goal_msg.pose.header.frame_id = "map";
        goal_msg.pose.pose.position.x = 1.5f;
        goal_msg.pose.pose.position.y = 0.0f;
        goal_msg.pose.pose.orientation.w = 1.0;

        auto send_goal_option = rclcpp_action::Client<NavigatorToPose>::SendGoalOptions();
        send_goal_option.goal_response_callback = [this](rclcpp_action::ClientGoalHandle<NavigatorToPose>::SharedPtr goal_handle){
            if(goal_handle)
            {
                RCLCPP_INFO(this->get_logger(),"目标点已被服务端接收");
            }
        };
        send_goal_option.feedback_callback = [this](rclcpp_action::ClientGoalHandle<NavigatorToPose>::SharedPtr goal_handle,NavigatorToPose::Feedback::ConstSharedPtr feedback){
            (void)goal_handle;
            RCLCPP_INFO(this->get_logger(),"反馈剩余距离:%f",feedback->distance_remaining);
        };
        send_goal_option.result_callback = [this](const rclcpp_action::ClientGoalHandle<NavigatorToPose>::WrappedResult& result){
            if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
            {
                RCLCPP_INFO(this->get_logger(),"处理成功");
            }
            else if(result.code == rclcpp_action::ResultCode::ABORTED)
            {
                RCLCPP_ERROR(this->get_logger(),"处理中断");
            }
            else if(result.code == rclcpp_action::ResultCode::CANCELED)
            {
                RCLCPP_WARN(this->get_logger(),"处理取消");
            }
            else{
                RCLCPP_ERROR(this->get_logger(),"未知处理原因");
            }
        };
        m_nav_client->async_send_goal(goal_msg,send_goal_option);
    }
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node = std::make_shared<NavToPoseClient>();
    node->send_goal();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}